{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# SmartProjectionRigFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "`SmartProjectionRigFactor<CAMERA>` is a generalization of `SmartProjectionPoseFactor` designed for multi-camera systems (rigs).\n",
        "Like other smart factors, it implicitly represents a 3D point landmark observed by multiple cameras.\n",
        "\n",
        "Key differences/features:\n",
        "- **Multi-Camera Rig:** Assumes a fixed rig configuration, where multiple cameras (`CAMERA` instances, which include fixed intrinsics and fixed extrinsics *relative to the rig's body frame*) are defined.\n",
        "- **Pose Variables:** Connects `Pose3` variables representing the pose of the **rig's body frame** in the world.\n",
        "- **Multiple Observations per Pose:** Allows multiple measurements associated with the *same* body pose key, but originating from different cameras within the rig.\n",
        "- **Camera Indexing:** Each measurement must be associated with both a body pose key and a `cameraId` (index) specifying which camera in the rig took the measurement.\n",
        "- **Fixed Calibration/Extrinsics:** The intrinsics and relative extrinsics of the cameras within the rig are assumed fixed.\n",
        "- **`CAMERA` Template:** Can be templated on various camera models (e.g., `PinholePose`, `SphericalCamera`), provided they adhere to the expected GTSAM camera concepts. **Important Note:** See the **Note on Template Parameter `CAMERA`** below.\n",
        "- **`Values` Requirement:** Requires `Pose3` objects (representing the body frame) in the `Values` container.\n",
        "- **Configuration:** Behavior controlled by `SmartProjectionParams`. **Note:** Currently (as of C++ header comment), only supports `HESSIAN` linearization mode and `ZERO_ON_DEGENERACY` mode.\n",
        "\n",
        "**Use Case:** Ideal for visual SLAM with a calibrated multi-camera rig (e.g., stereo rig, multi-fisheye system) where only the rig's pose is optimized.\n",
        "\n",
        "**Note on Template Parameter `CAMERA`:**\n",
        "While this factor is templated on `CAMERA` for generality, the current internal implementation for linearization has limitations. It implicitly assumes that `traits<CAMERA>::dimension` matches the optimized variable dimension (`Pose3::dimension`, which is 6).\n",
        "Consequently:\n",
        "- It works correctly with `CAMERA` types where `dimension == 6`, such as `PinholePose<CALIBRATION>` or `SphericalCamera`.\n",
        "- Using `CAMERA` types where `dimension != 6`, such as `PinholeCamera<CALIBRATION>` (dim = 6 + CalDim), **will cause compilation errors**.\n",
        "- **Recommendation:** For standard pinhole cameras in a fixed rig, **use `PinholePose<CALIBRATION>`** as the `CAMERA` type when defining the `CameraSet` for this factor.\n",
        "\n",
        "If you are using the factor, please cite:\n",
        "> **L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert**, \"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors\", Int. Conf. on Robotics and Automation (ICRA), 2014.\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/SmartProjectionRigFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "from gtsam import (\n",
        "    Values,\n",
        "    Point2,\n",
        "    Point3,\n",
        "    Pose3,\n",
        "    Rot3,\n",
        "    SmartProjectionParams,\n",
        "    LinearizationMode,\n",
        "    DegeneracyMode,\n",
        "    # Use PinholePose variant for wrapping\n",
        "    SmartProjectionRigFactorPinholePoseCal3_S2,\n",
        "    PinholePoseCal3_S2,\n",
        "    Cal3_S2,\n",
        ")\n",
        "from gtsam.symbol_shorthand import X  # Key for Pose3 variable (Body Pose)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_header_md"
      },
      "source": [
        "## Constructor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "constructor_desc_md"
      },
      "source": [
        "You create a `SmartProjectionRigFactor` by providing:\n",
        "1. A noise model for the 2D pixel measurements (typically `noiseModel.Isotropic`).\n",
        "2. A `CameraSet<CAMERA>` object defining the *fixed* rig configuration. Each `CAMERA` in the set contains its fixed intrinsics and its fixed pose relative to the rig's body frame (`body_P_camera`).\n",
        "3. Optionally, `SmartProjectionParams` to configure linearization and degeneracy handling. Remember the current restrictions (HESSIAN, ZERO_ON_DEGENERACY)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "constructor_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "SmartFactorRig: SmartProjectionRigFactor: \n",
            " SmartProjectionFactor\n",
            "linearizationMode: 0\n",
            "triangulationParameters:\n",
            "rankTolerance = 1\n",
            "enableEPI = 0\n",
            "landmarkDistanceThreshold = -1\n",
            "dynamicOutlierRejectionThreshold = -1\n",
            "useLOST = 0\n",
            "noise model\n",
            "\n",
            "result:\n",
            "no point, status = 1\n",
            "\n",
            "SmartFactorBase, z = \n",
            "  keys = { }\n"
          ]
        }
      ],
      "source": [
        "# Define the Camera Rig (using PinholePose)\n",
        "K = Cal3_S2(500, 500, 0, 320, 240)\n",
        "body_P_cam0 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, 0, 0))\n",
        "cam0 = PinholePoseCal3_S2(body_P_cam0, K)\n",
        "body_P_cam1 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, -0.1, 0))  # Baseline 0.1m\n",
        "cam1 = PinholePoseCal3_S2(body_P_cam1, K)\n",
        "\n",
        "rig_cameras = gtsam.CameraSetPinholePoseCal3_S2()\n",
        "rig_cameras.push_back(cam0)\n",
        "rig_cameras.push_back(cam1)\n",
        "\n",
        "# Noise model and parameters\n",
        "noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
        "smart_params = SmartProjectionParams(\n",
        "    linMode=LinearizationMode.HESSIAN, degMode=DegeneracyMode.ZERO_ON_DEGENERACY\n",
        ")\n",
        "\n",
        "# Create the Factor\n",
        "smart_factor = SmartProjectionRigFactorPinholePoseCal3_S2(\n",
        "    noise_model, rig_cameras, smart_params\n",
        ")\n",
        "smart_factor.print(\"SmartFactorRig: \")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "add_header_md"
      },
      "source": [
        "## `add(measurement, poseKey, cameraId)`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "add_desc_md"
      },
      "source": [
        "This method adds a single 2D measurement (`Point2`) associated with a specific camera in the rig and a specific body pose.\n",
        "- `measurement`: The observed pixel coordinates.\n",
        "- `poseKey`: The key (`Symbol` or `Key`) of the **body's `Pose3`** variable at the time of observation.\n",
        "- `cameraId`: The integer index of the camera within the `CameraSet` (provided during construction) that captured this measurement."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "id": "add_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Smart factor involves 2 measurements from 2 unique poses.\n"
          ]
        }
      ],
      "source": [
        "# --- Use Pre-calculated Valid Measurements ---\n",
        "# These measurements were calculated offline using:\n",
        "# gt_landmark = Point3(1.0, 0.5, 5.0)\n",
        "# gt_pose0 = Pose3()\n",
        "# gt_pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0))\n",
        "# And the rig defined above.\n",
        "\n",
        "z00 = Point2(400.0, 290.0)    # Measurement from Body Pose X(0), Camera 0\n",
        "z01 = Point2(350.0, 290.0)    # Measurement from Body Pose X(0), Camera 1\n",
        "z10 = Point2(372.787, 297.553) # Measurement from Body Pose X(1), Camera 0\n",
        "z11 = Point2(323.308, 297.674) # Measurement from Body Pose X(1), Camera 1\n",
        "# --------------------------------------------\n",
        "\n",
        "# 3. Add pre-calculated measurements\n",
        "smart_factor.add(z00, X(0), 0)\n",
        "smart_factor.add(z01, X(0), 1)\n",
        "smart_factor.add(z10, X(1), 0)\n",
        "smart_factor.add(z11, X(1), 1)\n",
        "\n",
        "print(f\"Smart factor involves {smart_factor.size()} measurements from {len(smart_factor.keys())} unique poses.\")\n",
        "# smart_factor.print(\"SmartFactorRig (with pre-calculated measurements): \") # Optional\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "inherited_header_md"
      },
      "source": [
        "## Inherited and Core Methods"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "error_header_md"
      },
      "source": [
        "### `error(Values values)`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "error_desc_md"
      },
      "source": [
        "Inherited from `SmartFactorBase`. Calculates the total reprojection error for the landmark.\n",
        "**Internal Process:**\n",
        "1. Retrieves the body `Pose3` estimates for all relevant keys from the `values` object.\n",
        "2. For each measurement, calculates the corresponding camera's world pose using the body pose and the fixed rig extrinsics (`world_P_sensor = world_P_body * body_P_camera`).\n",
        "3. Triangulates the 3D landmark position using these calculated camera poses and the stored 2D measurements.\n",
        "4. Reprojects the triangulated point back into each calculated camera view.\n",
        "5. Computes the sum of squared differences between the reprojections and the original measurements, weighted by the noise model.\n",
        "6. Handles degenerate cases (e.g., failed triangulation) based on the `degeneracyMode` set in `SmartProjectionParams`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "id": "error_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Triangulation Result Status: Status.VALID\n",
            "Total reprojection error (0.5 * sum(err^2/sigma^2)): 1316.4717\n"
          ]
        }
      ],
      "source": [
        "# Assuming smart_factor created and measurements added above\n",
        "\n",
        "values = Values()\n",
        "pose0 = Pose3() # Body at origin\n",
        "pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0)) # Body moved\n",
        "values.insert(X(0), pose0)\n",
        "values.insert(X(1), pose1)\n",
        "\n",
        "# Need to check triangulation first, as error calculation depends on it\n",
        "point_result = smart_factor.point(values)\n",
        "print(f\"Triangulation Result Status: {point_result.status}\")\n",
        "\n",
        "total_error = smart_factor.error(values)\n",
        "print(f\"Total reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n",
        "# Note: If degenerate and DegeneracyMode is ZERO_ON_DEGENERACY, error will be 0."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "point_header_md"
      },
      "source": [
        "### `point(Values values)`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "point_desc_md"
      },
      "source": [
        "Inherited from `SmartProjectionFactor`. Performs the internal triangulation based on the body poses in `values` and the fixed rig geometry.\n",
        "Returns a `TriangulationResult` object which contains:\n",
        "- The triangulated `Point3` (if successful).\n",
        "- A status indicating whether the triangulation was `VALID`, `DEGENERATE`, `BEHIND_CAMERA`, `OUTLIER`, or `FAR_POINT`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {
        "id": "point_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Triangulation Result Status: Status.VALID\n",
            "Triangulated Point: [0.94370846 0.79793704 7.63497051]\n"
          ]
        }
      ],
      "source": [
        "# Assuming smart_factor and values from the previous cell\n",
        "point_result = smart_factor.point(values)\n",
        "print(f\"Triangulation Result Status: {point_result.status}\")\n",
        "\n",
        "if point_result.valid():\n",
        "    triangulated_point = point_result.get()\n",
        "    print(f\"Triangulated Point: {triangulated_point}\")\n",
        "else:\n",
        "    print(\"Triangulation did not produce a valid point.\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "cameras_header_md"
      },
      "source": [
        "### `cameras(Values values)`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "cameras_desc_md"
      },
      "source": [
        "Inherited from `SmartFactorBase`. Computes and returns a `CameraSet<CAMERA>` containing the effective cameras corresponding to *each measurement*.\n",
        "For each measurement `i` associated with body pose key `k` and camera ID `cid`:\n",
        "1. Retrieves the body pose `world_P_body = values.atPose3(k)`.\n",
        "2. Retrieves the fixed relative pose `body_P_camera = rig_cameras.at(cid).pose()`.\n",
        "3. Computes the camera's world pose `world_P_camera = world_P_body * body_P_camera`.\n",
        "4. Creates a `CAMERA` object using this `world_P_camera` and the fixed intrinsics `rig_cameras.at(cid).calibration()`.\n",
        "The returned `CameraSet` contains these calculated cameras, one for each measurement added via `add()`."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {
        "id": "cameras_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Pose of camera for measurement 0 (Body X(0), Cam 0):\n",
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t: 0.1   0   0\n",
            "\n",
            "\n",
            "Pose of camera for measurement 1 (Body X(0), Cam 1):\n",
            "R: [\n",
            "\t1, 0, 0;\n",
            "\t0, 1, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t:  0.1 -0.1    0\n",
            "\n",
            "\n",
            "Pose of camera for measurement 2 (Body X(1), Cam 0):\n",
            "R: [\n",
            "\t0.995004, -0.0998334, 0;\n",
            "\t0.0998334, 0.995004, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t:     0.5995 0.00998334          0\n",
            "\n",
            "\n",
            "Pose of camera for measurement 3 (Body X(1), Cam 1):\n",
            "R: [\n",
            "\t0.995004, -0.0998334, 0;\n",
            "\t0.0998334, 0.995004, 0;\n",
            "\t0, 0, 1\n",
            "]\n",
            "t:   0.609484 -0.0895171          0\n",
            "\n",
            "\n"
          ]
        }
      ],
      "source": [
        "# Assuming smart_factor and values from previous cells\n",
        "\n",
        "calculated_cameras = smart_factor.cameras(values)\n",
        "\n",
        "# Print the world pose of each calculated camera\n",
        "print(f\"Pose of camera for measurement 0 (Body X(0), Cam 0):\\n{calculated_cameras.at(0).pose()}\\n\")\n",
        "print(f\"Pose of camera for measurement 1 (Body X(0), Cam 1):\\n{calculated_cameras.at(1).pose()}\\n\")\n",
        "print(f\"Pose of camera for measurement 2 (Body X(1), Cam 0):\\n{calculated_cameras.at(2).pose()}\\n\")\n",
        "print(f\"Pose of camera for measurement 3 (Body X(1), Cam 1):\\n{calculated_cameras.at(3).pose()}\\n\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "linearize_header_md"
      },
      "source": [
        "### `linearize(Values values)`"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "linearize_desc_md"
      },
      "source": [
        "Inherited from `SmartProjectionFactor`. Computes the linear approximation (GaussianFactor) of the factor at the linearization point defined by `values`.\n",
        "For `SmartProjectionRigFactor`, due to current implementation limitations, this **must** be configured via `SmartProjectionParams` to use `LinearizationMode.HESSIAN`.\n",
        "The resulting `RegularHessianFactor` connects the **unique body pose keys** involved in the measurements."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 10,
      "metadata": {
        "id": "linearize_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Linearized Factor (HessianFactor structure):\n",
            "Linear Factor: \n",
            " keys: x0(6) x1(6) \n",
            "Augmented information matrix: [\n",
            "\t255621, 1454.13, -31747.6, 636.066, -33103.6, 3605.16, -254669, 22279.1, 15195.9, 2671.95, 33001.7, -3605.16, -5437.65;\n",
            "\t1454.13, 9642.56, -1187.49, 1253.63, -198.336, -75.3949, -2405.75, -9411.71, 1088.32, -1227.56, 322.499, 75.3949, -653.552;\n",
            "\t-31747.6, -1187.49, 4048.22, -209.638, 4112.44, -437.73, 31729.4, -1770.15, -1992, -201.969, -4112.82, 437.73, 740.416;\n",
            "\t636.066, 1253.63, -209.638, 163.769, -83.6702, -3.45048, -757.87, -1182.15, 167.803, -154.598, 99.6018, 3.45048, -94.317;\n",
            "\t-33103.6, -198.336, 4112.44, -83.6702, 4287, -466.758, 32981.3, -2875.28, -1968.94, -344.734, -4273.93, 466.758, 704.833;\n",
            "\t3605.16, -75.3949, -437.73, -3.45048, -466.758, 51.9764, -3582.21, 409.075, 204.351, 50.0313, 464.082, -51.9764, -70.5256;\n",
            "\t-254669, -2405.75, 31729.4, -757.87, 32981.3, -3582.21, 253816, -21248.6, -15238.8, -2538.55, -32892.2, 3582.21, 5479.25;\n",
            "\t22279.1, -9411.71, -1770.15, -1182.15, -2875.28, 409.075, -21248.6, 11385.4, 332.508, 1463.29, "
          ]
        },
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "2742.9, -409.075, 142.514;\n",
            "\t15195.9, 1088.32, -1992, 167.803, -1968.94, 204.351, -15238.8, 332.508, 1007.53, 29.6019, 1975.86, -204.351, -387.999;\n",
            "\t2671.95, -1227.56, -201.969, -154.598, -344.734, 50.0313, -2538.55, 1463.29, 29.6019, 188.241, 327.577, -50.0313, 23.48;\n",
            "\t33001.7, 322.499, -4112.82, 99.6018, -4273.93, 464.082, -32892.2, 2742.9, 1975.86, 327.577, 4262.53, -464.082, -710.727;\n",
            "\t-3605.16, 75.3949, 437.73, 3.45048, 466.758, -51.9764, 3582.21, -409.075, -204.351, -50.0313, -464.082, 51.9764, 70.5256;\n",
            "\t-5437.65, -653.552, 740.416, -94.317, 704.833, -70.5256, 5479.25, 142.514, -387.999, 23.48, -710.727, 70.5256, 2632.94\n",
            "]\n"
          ]
        }
      ],
      "source": [
        "# Assuming smart_factor and values from previous cells\n",
        "\n",
        "# Check if triangulation succeeded before linearizing\n",
        "if not smart_factor.point(values).valid():\n",
        "    print(\"Cannot linearize: triangulation failed or degenerate.\")\n",
        "else:\n",
        "    linear_factor = smart_factor.linearize(values)\n",
        "\n",
        "    if linear_factor:\n",
        "        print(\"\\nLinearized Factor (HessianFactor structure):\")\n",
        "        linear_factor.print(\"Linear Factor: \")\n",
        "    else:\n",
        "        print(\"\\nLinearization failed (potentially due to triangulation degeneracy and params setting).\")\n",
        "\n",
        "# Note: The printed Hessian is often zero when ZERO_ON_DEGENERACY is used and triangulation fails."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "other_methods_header_md"
      },
      "source": [
        "### Other Inherited Methods"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "other_methods_desc_md"
      },
      "source": [
        "The factor also inherits standard methods from `NonlinearFactor` and `SmartFactorBase`:\n",
        "- **`keys()`**: Returns a `KeyVector` containing the **unique body pose keys** involved.\n",
        "- **`measured()`**: Returns a `Point2Vector` containing all the added 2D measurements.\n",
        "- **`dim()`**: Returns the dimension of the error vector (2 * number of measurements).\n",
        "- **`size()`**: Returns the number of measurements added.\n",
        "- **`print(s, keyFormatter)`**: Prints details about the factor.\n",
        "- **`equals(other, tol)`**: Compares two factors for equality."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 9,
      "metadata": {
        "id": "other_methods_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Keys: ['x0', 'x1']\n",
            "Number of Measurements (size): 2\n",
            "Dimension (dim): 8\n",
            "Measurements: [array([400., 290.]), array([350., 290.]), array([372.787, 297.553]), array([323.308, 297.674])]\n"
          ]
        }
      ],
      "source": [
        "# Assuming smart_factor from previous cells\n",
        "print(f\"Keys: {[gtsam.DefaultKeyFormatter(k) for k in smart_factor.keys()]}\")\n",
        "print(f\"Number of Measurements (size): {smart_factor.size()}\")\n",
        "print(f\"Dimension (dim): {smart_factor.dim()}\")\n",
        "print(f\"Measurements: {smart_factor.measured()}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
